# robomath - a module of trig and other odds and ends for WM to use

from __future__ import division
from math import *

def locate(object, side):
    """
    This function figures out the location of a stionary object (flag or goal)
    Input: list with flag or goal info, like: ['f', 'g', 'r', 't'], side of our team ('l' or 'r')
    Output: a touplet of the location (x, y) or 'None' if this isn't a flag or goal
    Note: maybe it would be faster to replace this with a dictionary of 55 objects?
          if we ever start running slow on speed, we might want to consider it,
          for now, just remember:
          
          'Guys, computers are *fast*.' -ML
    """
    # flag?
    if object[0] == 'f':
        # center flag ?
        if object[1] == 'c':
            x = 0.0
            if len(object) == 2: y = 0.0
            else: y = { 't':-34.0, 'b':34.0 }[object[2]]
        # penalty box flag ?
        elif object[1] == 'p':
            x = { 'l':-36.0, 'r':36.0 }[object[2]]
            y = { 't':-20.2, 'c':0.0, 'b': 20.2 }[object[3]]
        # goal flag ?
        elif object[1] == 'g':
            x = { 'l':-52.5, 'r':52.5 }[object[2]]
            y = { 't':-7.0, 'b':7.0 }[object[3]]
        # top ?
        elif object[1] == 't':
            y = -39.0
            if len(object) == 3: x = 0.0
            else: x = { 'l':-1, 'r':1 }[object[2]] * int(object[3])
        # bottom ?
        elif object[1] == 'b':
            y = 39.0
            if len(object) == 3: x = 0.0
            else: x = { 'l':-1, 'r':1 }[object[2]] * int(object[3])
        # left ?
        elif object[1] == 'l':
            if len(object) == 3:
                x = { '0':-57.5, 't':-52.5, 'b':-52.5 }[object[2]]
                y = { '0':0.0, 't':-34.0, 'b':34.0 }[object[2]]
            else:
                x = -57.5
                y = { 't':-1, 'b':1 }[object[2]] * int(object[3])
        # right?
        elif object[1] == 'r':
            if len(object) == 3:
                x = { '0':57.5, 't':52.5, 'b':52.5 }[object[2]]
                y = { '0':0.0, 't':-34.0, 'b':34.0 }[object[2]]
            else:
                x = 57.5
                y = { 't':-1, 'b':1 }[object[2]] * int(object[3])
        # mutant ninja flag ?
        else: print "WARNING: Unknown flag:" + str(object)
    # goal?
    elif object[0] == 'g':
        y = 0.0
        x = { 'l':-52.5, 'r':52.5 }[object[1]]
    # neither?
    else: return None
    
    if side == 'r':
        return (-x,y)
    else: return (x,y)

def choose(l, n):
    """Returns every possible n-sized subset of l, ordered like l"""
    if n == 1:
        return map(lambda x:[x], l)
    else:
        s = []
        for x in range(len(l)):
            s += [[l[x]]+y for y in choose(l[x+1:], n-1)]
        return s

def calculate_location(objs):
    """ used by WM to find the location of the player
    Input: a list of tuples (x,y,d,theta) where x and y are the coordinates of,
           each object, theta is the angle at which it is seen, and d is the
           distance in which it lies.
    Output: a tuple (x,y) that is the location of the player
    """
    intersects = []
    
    # choose all pairs of passed objects
    for (a, b) in choose(objs, 2):
        intersects += [x for x in intersect(a, b)]
    
    # find the average of them all
    avgIntersect = [0.0,0.0]
    
    for x in intersects:
        avgIntersect[0] += x[0]
        avgIntersect[1] += x[1]
    
    if len(intersects) == 0:
        print "Warning (robomath::triangulate): not enough good flags in sight - objs:", objs
        return None
    
    avgIntersect[0] /= len(intersects)
    avgIntersect[1] /= len(intersects)
    
    return (tuple(avgIntersect))

def intersect(P0, P1):
    """ find your location in relation to two points
    Input: P0, P1 - lists of the two points, the direction they're in, and the distance
    Output: tuple of the two points or 'None'
    """
    #order left-to-right
    if P0[3]>P1[3]: P0, P1 = P1, P0
    
    # set up some vars
    x1, y1, d1, t1 = P0[:4]
    x2, y2, d2, t2 = P1[:4]
    dx = x2-x1
    dy = y2-y1
    
    if dx==0: mangle = pi/2
    else: mangle = atan(dy/dx)
    
    d = (dx**2+dy**2)**0.5
    theta = radians(t2-t1)
    
    # fix this later
    if sin(theta) == 0: return ()
    
    # first, let's assume theta is correct and d1 (distance to point 1) is correct
    # this is a relatively simple trig problem... 
    
    # theta is probably acute, but check just in case...
    if theta < 90:
        # what kind of triangle is it?
        # acute/obtuse?
        if d1*sin(theta) < d:
            # now there are two possible solutions for the angle at p2
            phi1 = asin(d1*sin(theta)/d)    # is one
            phi2 = pi - phi1                # is the other
            
            # but we're looking for the angle at p1...
            alpha1 = pi - phi1 - theta
            alpha2 = pi - phi2 - theta
            
            # only one of these is right, so we *compute* the second side
            c1d2 = d*sin(alpha1)/sin(theta)
            c2d2 = d*sin(alpha2)/sin(theta)
            
            if abs(c1d2-d2) < abs(c2d2-d2):
                alpha = alpha1
                phi = phi1
            else:
                alpha = alpha2
                phi = phi2
            
            # maybe a little of the magic ingredient: pi?
            if dx < 0: slope = mangle + alpha + pi
            # or maybe not... it's like magic, dude
            elif dx > 0: slope = mangle + alpha
            else:
                if dy > 0: slope = mangle + alpha
                else: slope = mangle + alpha + pi
            
        # right?
        elif d1*sin(theta) == d: return ()
        # not a triangle
        else: return ()
        
        #print theta, alpha, phi, slope, theta+alpha+phi, mangle
        solutions = ((x1+d1*cos(slope), y1+d1*sin(slope)),)

    else: return ()
    
    # print solutions
    return solutions

def calculate_direction(location, flags):
    """This function finds the angle of the player in relation to the x axis
    Input: location tuple - (x,y)
           flags tuple(s) - ((x,y,d,th[, ...])+)
    """
    angles = [angle_to(flag, location) for flag in flags]
    
    # a fix for averaging -pi/2 and +3pi/2 to be pi/2...
    if True in [angle > 180 for angle in angles]:
        for i in range(len(angles)):
            if angles[i] < 0:
                angles[i] += 360
    
    if (len(flags) - angles.count(None)) == 0:
        print "Warning (robomath::find_angle): no two flags in sight..."
        return None

    avg = sum(angles) / (len(flags) - angles.count(None))

    if avg < 0:
        return radians(avg + 360)
    else:
        return radians(avg)
    
def angle_to(flag, location):
    dx = flag[0] - location[0]
    dy = flag[1] - location[1]
    
    #print location, flag, dy, dx
        
    if dx > 0: return degrees(atan(dy/dx))-flag[3]
    elif dx < 0: return degrees(atan(dy/dx)+pi)-flag[3]
    else:
        if dy > 0: return 90-flag[3]
        elif dy < 0: return 270-flag[3]
        else: return None

def angle_from_to(object, target):
    """This function returns the angle of the vector between object and target
    object/target: (x,y)
    returns: [0, 2pi)
    """
    dx = target[0] - object[0]
    dy = target[1] - object[1]
    
    if dx > 0:
        a = atan(dy/dx)
        if a < 0: a += 2*pi
        return a
    elif dx < 0:
        return atan(dy/dx) + pi
    else:
        if dy > 0:
            return pi/2
        elif dy < 0:
            return 3*pi/2
        else: return 0

def rect_to_polar(vec, dist=None):
    """This function converts a rectangular vector (x,y) to polar form (theta, dist)"""
    (x,y) = vec
    return ( angle_from_to((0,0), vec), dist or sqrt(x*x+y*y) )
    
def calculate_velocity(objs, loc):
    """This function calculates velocity"""
#    for x in objs:
#        for y in x:
#            print y,
#        print ""
#    print '---'

    vx = []
    vy = []
    
    for obj in objs:
        # if no chng. in dist, no info
        if obj[4] == 0.0: continue
        
        # find rel_vector + velocity
        dist_prime = obj[2] + obj[4]
        theta_prime = angle_from_to(obj, loc) + obj[5]
        
        rel_x_prime = dist_prime * cos(theta_prime)
        rel_y_prime = dist_prime * sin(theta_prime)
        
        vx += [rel_x_prime-loc[0]]
        vy += [rel_y_prime-loc[1]]
    
    try:
        vx = reduce(lambda x,y:x+y, vx, 0)/len(vx)
        vy = reduce(lambda x,y:x+y, vy, 0)/len(vy)
    except ZeroDivisionError:
        vx = vy = None
            
    return (vx, vy)
    
